#include "PlanePointRansacMatcherI.h"
#include "g2o/types/slam3d/vertex_se3_quat.h"
#include "graph/VertexPlane.h"

#include "Point.h"
#include <vector>
#include <algorithm>

#include "AssociationSet.h"


void HSVtoRGB( int  *r, int *g,int *b, int h, int s, int v )
{
	int f;
	long p, q, t;
	if( s == 0 )
	{
		*r = *g = *b = v;
		return;
	}
 
        f = ((h%60)*255)/60;
        
        h /= 60;
 
        p = (v * ( 256 - s))/256;
        q = (v * ( 256 - (s * f)/256 ))/256;
        t = (v * ( 256 - (s * ( 256 - f ))/256))/256;
 
        switch( h ) {
                case 0:
                        *r = v;
                        *g = t;
                        *b = p;
                        break;
                case 1:
                        *r = q;
                        *g = v;
                        *b = p;
                        break;
                case 2:
                        *r = p;
                        *g = v;
                        *b = t;
                        break;
                case 3:
                        *r = p;
                        *g = q;
                        *b = v;
                        break;
                case 4:
                        *r = t;
                        *g = p;
                        *b = v;
                        break;
                default:
                        *r = v;
                        *g = p;
                        *b = q;
                        break;
        }
}

using namespace std;
using namespace Eigen;

PlanePointRansacMatcherI::PlanePointRansacMatcherI()
{
	name = "PlanePointRansacMatcherI";
	ROS_INFO("new PlanePointRansacMatcherI");

	graphoptimizer.setMethod(g2o::SparseOptimizer::LevenbergMarquardt);
	graphoptimizer.setVerbose(false);
	g2o::BlockSolver_6_3::LinearSolverType * linearSolver = new g2o::LinearSolverPCG<g2o::BlockSolver_6_3::PoseMatrixType>();
	g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(&graphoptimizer,linearSolver);
	graphoptimizer.setSolver(solver_ptr);
	
	vertexSE3_src=0;
	vertexSE3_dst=0;

}

PlanePointRansacMatcherI::~PlanePointRansacMatcherI()
{
	ROS_INFO("delete PlanePointRansacMatcherI");
}

void PlanePointRansacMatcherI::update(){
}

const bool debugg_PlanePointRansacMatcherI_associate = true;
void PlanePointRansacMatcherI::associate(FrameNode * src, FrameNode * dst)
{
	if(debugg_PlanePointRansacMatcherI_associate){}

	
	AssociationSet first(src,dst);
	int len = first.len;
	std::vector<AssociationSet> * association_search_space = new std::vector<AssociationSet>();
	make_heap (association_search_space->begin(),association_search_space->end());
	association_search_space->push_back(first);
	push_heap(association_search_space->begin(),association_search_space->end());
	
	int iteration_counter = 0;
	
	const int hashmap_size = 1024;
	std::vector<char *> ** hashmap = new std::vector<char *>*[hashmap_size];
	for(int hm = 0; hm < hashmap_size; hm++){hashmap[hm] = new std::vector<char *>();}
	
	struct timeval start, end;
	gettimeofday(&start, NULL);
	
	while(!association_search_space->empty())
	{
		AssociationSet inspection = association_search_space->front();
		pop_heap(association_search_space->begin(),association_search_space->end());
		association_search_space->pop_back();
	/*
		for(int i = 0; i < inspection.selected_associations.size(); i++)
		{
			Association *  tmp_asso = inspection.selected_associations.at(i);
			if(tmp_asso->type == 0){
				PlaneAssociation * tmp_pa = (PlaneAssociation * )tmp_asso;
				char buffer [50];
				sprintf (buffer, "plane%i",tmp_pa->p1->id);
				string * plane_name1 = new string(buffer);
			
				sprintf (buffer, "plane%i",tmp_pa->p2->id);
				string * plane_name2 = new string(buffer);
			
				ROS_INFO("%s <---> %s",plane_name1->c_str(),plane_name2->c_str());
				//viewer->spinOnce (1000);
				float fr = (rand()%1000)/1000.0f;
				float fg = (rand()%1000)/1000.0f;
				float fb = (rand()%1000)/1000.0f;
				viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR,fr,fg,fb,*plane_name1);
				viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR,fr,fg,fb,*plane_name2);
			}
		}

		ROS_INFO("__________________________________________________%i____________________________________________________________",iteration_counter);

		struct timeval start, end;
		gettimeofday(&start, NULL);
		gettimeofday(&end, NULL);
		float timespent = (end.tv_sec*1000000+end.tv_usec-(start.tv_sec*1000000+start.tv_usec))/1000000.0f;
		
		for(int i = 0; timespent < 1; i++){
			viewer->spinOnce (10);
			gettimeofday(&end, NULL);
			timespent = (end.tv_sec*1000000+end.tv_usec-(start.tv_sec*1000000+start.tv_usec))/1000000.0f;
		}
		
		for(int i = 0; i < inspection.selected_associations.size(); i++)
		{
			Association *  tmp_asso = inspection.selected_associations.at(i);
			if(tmp_asso->type == 0){
				PlaneAssociation * tmp_pa = (PlaneAssociation * )tmp_asso;
				char buffer [50];
				sprintf (buffer, "plane%i",tmp_pa->p1->id);
				string * plane_name1 = new string(buffer);
			
				sprintf (buffer, "plane%i",tmp_pa->p2->id);
				string * plane_name2 = new string(buffer);

				//viewer->spinOnce (1000);
				viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR,255,0,255,*plane_name1);
				viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR,255,0,255,*plane_name2);
			}
		}
	*/
		std::vector<AssociationSet> exploration = inspection.explore();
		for(unsigned int i = 0; i < exploration.size(); i++)
		{
			char * hash = exploration.at(i).getHashValue();
			int sum = 0;
			for(int j = 0; j < 1+len/8; j++)
			{
				if(hash[j] < 0)	{sum -= hash[j];}
				else			{sum += hash[j];}
			}
			bool in_hashmap = false;
			sum = sum % hashmap_size;
			for(unsigned int j = 0; j < hashmap[sum]->size();j++)
			{
				bool same = true;
				char * hash2 = hashmap[sum]->at(j);
				for(int k = 0; k < 1+len/8; k++){same = same && (hash[k] == hash2[k]);}
				if(same){in_hashmap = true;break;}
			}
			if(!in_hashmap)
			{
				//ROS_INFO("not in hashmap");
				hashmap[sum]->push_back(hash);
				association_search_space->push_back(exploration.at(i));
			}
		}
		iteration_counter++;
	}
	gettimeofday(&end, NULL);
	float timespent = (end.tv_sec*1000000+end.tv_usec-(start.tv_sec*1000000+start.tv_usec))/1000000.0f;
	ROS_INFO("timespent: %f",timespent);
	ROS_INFO("iteration_counter = %i",iteration_counter);
	exit(0);
}

const bool debugg_PlanePointRansacMatcherI = true;
Transformation * PlanePointRansacMatcherI::getTransformation(FrameNode * src, FrameNode * dst)
{
	ROS_INFO("PlanePointRansacMatcherI::getTransformation");
	associate(src,dst);

	
	Eigen::Affine3f eigenTransform(Eigen::Matrix4f::Identity());
	Eigen::Quaternionf eigenRotation(eigenTransform.rotation());
	
	g2o::SE3Quat poseSE3_src(
		Eigen::Quaterniond(eigenRotation.w(), eigenRotation.x(), eigenRotation.y(), eigenRotation.z()),
		Eigen::Vector3d(eigenTransform(0,3), eigenTransform(1,3), eigenTransform(2,3)));
	
	g2o::SE3Quat poseSE3_dst(
		Eigen::Quaterniond(eigenRotation.w(), eigenRotation.x(), eigenRotation.y(), eigenRotation.z()),
		Eigen::Vector3d(eigenTransform(0,3), eigenTransform(1,3), eigenTransform(2,3)));
	if(vertexSE3_src!=0)
	{
		graphoptimizer.removeVertex(vertexSE3_src);
		delete vertexSE3_src;
	}
	
	if(vertexSE3_dst!=0)
	{
		graphoptimizer.removeVertex(vertexSE3_src);
		delete vertexSE3_dst;
	}
	vertexSE3_src = new g2o::VertexSE3();
	vertexSE3_src->setId(0);
	vertexSE3_src->estimate() = poseSE3_src;
	vertexSE3_src->setFixed(true);
	vertexSE3_dst = new g2o::VertexSE3();
	vertexSE3_dst->setId(1);
	vertexSE3_dst->estimate() = poseSE3_dst;
	vertexSE3_dst->setFixed(true);
	
	graphoptimizer.addVertex(vertexSE3_src);
	graphoptimizer.addVertex(vertexSE3_dst);
	
	//exit(0);
	
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr src_cloud;
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr dst_cloud;
	Eigen::Matrix4f display_transformationMat = Eigen::Matrix4f::Identity();
	display_transformationMat(0,3)=5000;
	
	if(debugg_PlanePointRansacMatcherI)
	{
		ROS_INFO("PlanePointRansacMatcherI::getTransformation(%i,%i)",src->frame->id_number,dst->frame->id_number);
		
		src_cloud = src->frame->getCloud();
		dst_cloud = dst->frame->getCloud();

		pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> src_rgb(src_cloud);
		viewer->addPointCloud<pcl::PointXYZRGB> (src_cloud, src_rgb, "src_cloud");
		viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "src_cloud");
		
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr result2 		(new pcl::PointCloud<pcl::PointXYZRGB>);
		pcl::transformPointCloud (*dst_cloud, *result2, display_transformationMat);
		pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> dst_rgb(result2);
		viewer->addPointCloud<pcl::PointXYZRGB> (result2, dst_rgb, "dst_cloud");
		viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "dst_cloud");
	}
	
	vector<Plane *> * src_planes = &(src->frame->planes);
	vector<Plane *> * dst_planes = &(dst->frame->planes);
	string ** plane_names_src;
	string ** plane_names_dst;
	if(debugg_PlanePointRansacMatcherI)
	{
		float plane_scale = 5.0f*2.5f;
		plane_names_src = new string*[src_planes->size()];
		plane_names_dst = new string*[dst_planes->size()];
		for(unsigned int i = 0; i < src_planes->size(); i++)
		{
			Plane * plane = src_planes->at(i);
			char buffer [50];
			sprintf (buffer, "plane%i",plane->id);
			string * plane_name = new string(buffer);
			plane_names_src[i] = plane_name;
			
			pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
			tmp_cloud->width = 100;
			tmp_cloud->height = 100;
			tmp_cloud->is_dense = false;
			tmp_cloud->points.resize(tmp_cloud->width*tmp_cloud->height);
	
			int nr_points = 0;
			ROS_INFO("tmp_cloud2->width/2 = %f",float(tmp_cloud->width/2));
			ROS_INFO("-tmp_cloud2->width/2 = %f",-float(tmp_cloud->width/2));
			ROS_INFO("tmp_cloud2->height/2 = %f",float(tmp_cloud->height/2));
			ROS_INFO("-tmp_cloud2->height/2 = %f",-float(tmp_cloud->height/2));
			for(float w = -float(tmp_cloud->width/2) ; w < float(tmp_cloud->width/2); w++)
			{
				for(float h = -float(tmp_cloud->width/2) ; h < float(tmp_cloud->width/2); h++)
				{
					tmp_cloud->points[nr_points].x = plane->point_x+plane_scale*w*plane->plane_points(1,0)+plane_scale*h*plane->plane_points(2,0);
					tmp_cloud->points[nr_points].y = plane->point_y+plane_scale*w*plane->plane_points(1,1)+plane_scale*h*plane->plane_points(2,1);
					tmp_cloud->points[nr_points].z = plane->point_z+plane_scale*w*plane->plane_points(1,2)+plane_scale*h*plane->plane_points(2,2);
					tmp_cloud->points[nr_points].r = (unsigned char)255;
					tmp_cloud->points[nr_points].g = (unsigned char)0;
					tmp_cloud->points[nr_points].b = (unsigned char)255;
					nr_points++;
				}
			}
			pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> plane_rgb(tmp_cloud);
			viewer->addPointCloud<pcl::PointXYZRGB> (tmp_cloud, plane_rgb, *plane_name);
			viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, *plane_name);
			viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5f,  *plane_name);
			viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR,255,0,255,*plane_name);
		}
		
		for(unsigned int i = 0; i < dst_planes->size(); i++)
		{
			Plane * plane = dst_planes->at(i);
			char buffer [50];
			//float plane_scale = 5.0f*2.5f;
			sprintf (buffer, "plane%i",plane->id);
			string * plane_name = new string(buffer);
			plane_names_dst[i] = plane_name;
			
			pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
			tmp_cloud->width = 100;
			tmp_cloud->height = 100;
			tmp_cloud->is_dense = false;
			tmp_cloud->points.resize(tmp_cloud->width*tmp_cloud->height);
	
			int nr_points = 0;
			ROS_INFO("tmp_cloud2->width/2 = %f",float(tmp_cloud->width/2));
			ROS_INFO("-tmp_cloud2->width/2 = %f",-float(tmp_cloud->width/2));
			ROS_INFO("tmp_cloud2->height/2 = %f",float(tmp_cloud->height/2));
			ROS_INFO("-tmp_cloud2->height/2 = %f",-float(tmp_cloud->height/2));
			for(float w = -float(tmp_cloud->width/2) ; w < float(tmp_cloud->width/2); w++)
			{
				for(float h = -float(tmp_cloud->width/2) ; h < float(tmp_cloud->width/2); h++)
				{
					tmp_cloud->points[nr_points].x = plane->point_x+plane_scale*w*plane->plane_points(1,0)+plane_scale*h*plane->plane_points(2,0);
					tmp_cloud->points[nr_points].y = plane->point_y+plane_scale*w*plane->plane_points(1,1)+plane_scale*h*plane->plane_points(2,1);
					tmp_cloud->points[nr_points].z = plane->point_z+plane_scale*w*plane->plane_points(1,2)+plane_scale*h*plane->plane_points(2,2);
					tmp_cloud->points[nr_points].r = (unsigned char)255;
					tmp_cloud->points[nr_points].g = (unsigned char)0;
					tmp_cloud->points[nr_points].b = (unsigned char)255;
					nr_points++;
				}
			}
			pcl::PointCloud<pcl::PointXYZRGB>::Ptr result2 		(new pcl::PointCloud<pcl::PointXYZRGB>);
			pcl::transformPointCloud (*tmp_cloud, *result2, display_transformationMat);
			
			pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> plane_rgb(result2);
			viewer->addPointCloud<pcl::PointXYZRGB> (result2, plane_rgb, *plane_name);
			viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3,  *plane_name);
			viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5f,  *plane_name);
			viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR,255,0,255,*plane_name);
		}
		viewer->spinOnce (1000);
	}
	
	associate(src,dst);
	exit(0);
	
	
	
	
	
	
	
	
	int ransac_iters = 1000;
	int nr_parts_matching = 3;
	int * parts_matching_src = new int[nr_parts_matching];
	int * parts_matching_dst = new int[nr_parts_matching];
	
	float * pr = new float[nr_parts_matching];
	float * pg = new float[nr_parts_matching];
	float * pb = new float[nr_parts_matching];
	
	for(int i = 0; i < nr_parts_matching; i++)
	{
		pr[i] = ((float)(rand() % 10000))/10000.0f;
		pg[i] = ((float)(rand() % 10000))/10000.0f;
		pb[i] = ((float)(rand() % 10000))/10000.0f;
		ROS_INFO("%f     %f     %f",pr[i],pg[i],pb[i]);
	}
	
	for(int iter = 0; iter < ransac_iters; iter)
	{
		for(int i = 0; i < nr_parts_matching; i++)
		{
			int src_plane = rand() % src_planes->size();
			int dst_plane = rand() % dst_planes->size();
			for(int j = 0; j < i; j++)
			{
				while(src_plane == parts_matching_src[j]){src_plane = rand() % src_planes->size();}
				while(dst_plane == parts_matching_dst[j]){dst_plane = rand() % dst_planes->size();}
			}
			parts_matching_src[i] = src_plane;
			parts_matching_dst[i] = dst_plane;
		}

		if(debugg_PlanePointRansacMatcherI)
		{
			for(unsigned int i = 0; i < src_planes->size(); i++){
				viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR,0,1,0,*plane_names_src[i]);
			}
			for(unsigned int i = 0; i < dst_planes->size(); i++){
				viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR,0,1,0,*plane_names_dst[i]);
			}
			for(int i = 0; i < nr_parts_matching; i++)
			{
				viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR,pr[i],pg[i],pb[i],*plane_names_src[parts_matching_src[i]]);
				viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR,pr[i],pg[i],pb[i],*plane_names_dst[parts_matching_dst[i]]);
			}
			viewer->spinOnce (1000);
			usleep(100000);
			/*
			viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR,0,255,0,*plane_names_src[src_plane_random]);
			viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR,0,255,0,*plane_names_dst[dst_plane_random]);
			for(int i = 0; i < 10; i++)
			{
				viewer->spinOnce (100);
				usleep(10000);
			}
			viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR,255,0,255,*plane_names_src[src_plane_random]);
			viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR,255,0,255,*plane_names_dst[dst_plane_random]);
			*/
		}
	}
	delete parts_matching_src;
	delete parts_matching_dst;
	delete pr;
	delete pg;
	delete pb;
	
	if(debugg_PlanePointRansacMatcherI)
	{
		while (!viewer->wasStopped ())
		{
			ROS_INFO("...");
			viewer->spinOnce (100);
			usleep(100000);
		}
	}
	ROS_INFO("DONE with transformation");
	exit(0);
	return 0;
}
/*
	IplImage* img_combine;
	int sx;
	int sy;
	if(debugg_PlanePointRansacMatcherI)
	{
		IplImage* img_a = cvLoadImage( src->frame->rgb_path.c_str(), 1);
		IplImage* img_b = cvLoadImage( dst->frame->rgb_path.c_str(), 1);
	
		sx = img_a->width;
		sy = img_a->height;
		
		img_combine = cvCreateImage(cvSize(2*sx,sy), IPL_DEPTH_8U, 3);
		for (int i = 0; i < sx; i++)
		{
			for (int j = 0; j < sy; j++)
			{
				img_combine->imageData[3 * (j * (2*sx) + (sx+i)) + 0] = img_a->imageData[3 * (j * sx + i) + 0];
				img_combine->imageData[3 * (j * (2*sx) + (sx+i)) + 1] = img_a->imageData[3 * (j * sx + i) + 1];
				img_combine->imageData[3 * (j * (2*sx) + (sx+i)) + 2] = img_a->imageData[3 * (j * sx + i) + 2];
			}
		}
	
		for (int i = 0; i < sx; i++)
		{
			for (int j = 0; j < sy; j++)
			{
				img_combine->imageData[3 * (j * (2*sx) + (i)) + 0] = img_b->imageData[3 * (j * sx + i) + 0];
				img_combine->imageData[3 * (j * (2*sx) + (i)) + 1] = img_b->imageData[3 * (j * sx + i) + 1];
				img_combine->imageData[3 * (j * (2*sx) + (i)) + 2] = img_b->imageData[3 * (j * sx + i) + 2];
			}
		}
		cvReleaseImage( &img_a );
		cvReleaseImage( &img_b );
	}
	
	Transformation * transformation = new Transformation();
	transformation->src = src;
	transformation->dst = dst;
	transformation->weight = 100;
	transformation->level = 1;
	
	KeyPointSet * src_keypointset 	= src->frame->keypoints;
	KeyPointSet * dst_keypointset 	= dst->frame->keypoints;
	KeyPoint ** src_keypoints	 	= src->frame->keypoints->valid_key_points;
	KeyPoint ** dst_keypoints 		= dst->frame->keypoints->valid_key_points;

	int src_nr_points = src_keypointset->nr_valid_points;
	int dst_nr_points = dst_keypointset->nr_valid_points;

	float ** surf_distances = new float*[src_nr_points];
	for(int i = 0; i < src_nr_points;i++)
	{
		surf_distances[i] = new float[dst_nr_points];		
		for(int j = 0; j < dst_nr_points;j++)
		{
			FeatureDescriptor * descriptorA = src_keypoints[i]->surf_descriptor;
			FeatureDescriptor * descriptorB = dst_keypoints[j]->surf_descriptor;
			surf_distances[i][j] = descriptorA->distance(descriptorB);
		}
	}

	
	float * pos_src_x = new float[src_nr_points];
	float * pos_src_y = new float[src_nr_points];
	float * pos_src_z = new float[src_nr_points];
	
	float * pos_dst_x = new float[dst_nr_points];
	float * pos_dst_y = new float[dst_nr_points];
	float * pos_dst_z = new float[dst_nr_points];
	
	Eigen::Matrix4f transformationMat = Eigen::Matrix4f::Identity();
	
	for(int i = 0; i < src_nr_points;i++)
	{
		pos_src_x[i] = src_keypoints[i]->point->x;//position(0);
		pos_src_y[i] = src_keypoints[i]->point->y;
		pos_src_z[i] = src_keypoints[i]->point->z;;
		if(debugg_PlanePointRansacMatcherI){cvCircle(img_combine,cvPoint(src_keypoints[i]->point->w+sx, src_keypoints[i]->point->h), 5,cvScalar(0, 0, 255, 0),2, 8, 0);}
	}
	for(int i = 0; i < dst_nr_points;i++)
	{
		pos_dst_x[i] = dst_keypoints[i]->point->x;
		pos_dst_y[i] = dst_keypoints[i]->point->y;
		pos_dst_z[i] = dst_keypoints[i]->point->z;
		if(debugg_PlanePointRansacMatcherI){cvCircle(img_combine,cvPoint(dst_keypoints[i]->point->w, dst_keypoints[i]->point->h), 5,cvScalar(0, 0, 255, 0),2, 8, 0);}
	}
	if(debugg_PlanePointRansacMatcherI){
		cvNamedWindow("combined image", CV_WINDOW_AUTOSIZE );
		cvShowImage("combined image", img_combine);
		cvWaitKey(0);
	}
	float surf_theshold = 0.2;
	int * src_matches 				= new int[src_nr_points];
	int * dst_matches 				= new int[dst_nr_points];
	for(int i = 0; i < src_nr_points;i++)
	{
		src_matches[i] = -1;
		float best_value = 9999999;
		for(int j = 0; j < dst_nr_points;j++)
		{
			if(surf_distances[i][j]<best_value)
			{
				best_value = surf_distances[i][j];
				src_matches[i] = j;
			}
		}
		ROS_INFO("BEST src MATCH: %i -> %i  = %f",i,src_matches[i],best_value);
	}
	for(int j = 0; j < dst_nr_points;j++)
	{
		dst_matches[j] = -1;
		float best_value = 9999999;
		for(int i = 0; i < src_nr_points;i++)
		{
			if(surf_distances[i][j]<best_value)
			{
				best_value = surf_distances[i][j];
				dst_matches[j] = i;
			}
		}
		ROS_INFO("BEST dst MATCH: %i -> %i  = %f",j,dst_matches[j],best_value);
	}

	pcl::TransformationFromCorrespondences tfc;	
	KeyPoint ** src_match_id = new KeyPoint*[src_nr_points];
	KeyPoint ** dst_match_id = new KeyPoint*[src_nr_points];
		
	//int * src_matches_id = new int[src_nr_points];
	//int * dst_matches_id = new int[src_nr_points];
	int nr_matches = 0;
	
	for(int i = 0; i < src_nr_points;i++)
	{
		if(i == dst_matches[src_matches[i]] && surf_distances[i][src_matches[i]] < surf_theshold){
			ROS_INFO("\n\n\nsrc MATCH: %i -> %i  = %f",i,src_matches[i],surf_distances[i][src_matches[i]]);
			KeyPoint * src_kp  = src->frame->keypoints->valid_key_points[i];
			KeyPoint * dst_kp  = dst->frame->keypoints->valid_key_points[src_matches[i]];
			ROS_INFO("\n___________________________\nsrc data: ");
			src_kp->print();
			ROS_INFO("\n___________________________\ndst data: ");
			dst_kp->print();
			ROS_INFO("done print()");
			src_match_id[nr_matches] = src_kp;
			dst_match_id[nr_matches] = dst_kp; 
			nr_matches++;
			if(debugg_PlanePointRansacMatcherI){
			//	ROS_INFO("Src w:h = %i, %i",src_match_id[nr_matches]->point->w,src_match_id[nr_matches]->point->h);
			//	ROS_INFO("dst w:h = %i, %i",dst_match_id[nr_matches]->point->w,dst_match_id[nr_matches]->point->h);
				//cvLine(img_combine,cvPoint(src_kp->point->w,src_kp->point->h),cvPoint(200, 200),cvScalar(0, 255, 0, 0),1, 8, 0);
				//cvLine(img_combine,cvPoint(dst_kp->point->w + sx,dst_kp->point->h),cvPoint(200 + sx, 200),cvScalar(0, 255, 0, 0),1, 8, 0);
				cvLine(img_combine,cvPoint(dst_kp->point->w ,dst_kp->point->h),cvPoint(src_kp->point->w + sx,src_kp->point->h),cvScalar(0, 255, 0, 0),1, 8, 0);
				//cvLine(img_combine,cvPoint(src_match_id[nr_matches]->point->w,src_match_id[nr_matches]->point->h),cvPoint(dst_match_id[nr_matches]->point->w + sx, dst_match_id[nr_matches]->point->h),cvScalar(0, 255, 0, 0),1, 8, 0);
			}
		}
	}

	if(debugg_PlanePointRansacMatcherI){
		ROS_INFO("debugging");
		//cvNamedWindow("combined image", CV_WINDOW_AUTOSIZE );
		//cvShowImage("combined image", img_combine);
		//cvWaitKey(0);
	}

	if(nr_matches >= 3)
	{
		KeyPoint * src_point;
		KeyPoint * dst_point;
		int best_fit 		= 0;
		int* best_indexes	= new int[nr_matches];
		for(int i = 0; i < nr_iter; i++)
		{
			int match_a = rand() % nr_matches;
			int match_b = rand() % nr_matches;
			int match_c = rand() % nr_matches;
			while(match_b == match_a){match_b = rand() % nr_matches;}
			while(match_c == match_b || match_c == match_a){match_c = rand() % nr_matches;}
			
			tfc.reset();
			tfc.add(src_match_id[match_a]->point->pos, dst_match_id[match_a]->point->pos);
			tfc.add(src_match_id[match_b]->point->pos, dst_match_id[match_b]->point->pos);
			tfc.add(src_match_id[match_c]->point->pos, dst_match_id[match_c]->point->pos);
			
			transformationMat = tfc.getTransformation().matrix();
			float mat00 = transformationMat(0,0);
			float mat01 = transformationMat(0,1);
			float mat02 = transformationMat(0,2);
			float mat03 = transformationMat(0,3);
			float mat10 = transformationMat(1,0);
			float mat11 = transformationMat(1,1);
			float mat12 = transformationMat(1,2);
			float mat13 = transformationMat(1,3);
			float mat20 = transformationMat(2,0);
			float mat21 = transformationMat(2,1);
			float mat22 = transformationMat(2,2);
			float mat23 = transformationMat(2,3);
			//float mat30 = transformationMat(3,0);
			//float mat31 = transformationMat(3,1);
			//float mat32 = transformationMat(3,2);
			//float mat33 = transformationMat(3,3);
			int nr_good_fitts = 0;

			for(int j = 0; j < nr_matches; j++)
			{
				src_point = src_match_id[j];
				dst_point = dst_match_id[j];
			
				float x = src_point->point->x;
				float y = src_point->point->y;
				float z = src_point->point->z;
				
				float dx = x*mat00+y*mat01+z*mat02+mat03-dst_point->point->x;
				float dy = x*mat10+y*mat11+z*mat12+mat13-dst_point->point->y;
				float dz = x*mat20+y*mat21+z*mat22+mat23-dst_point->point->z;

				if(dx*dx + dy*dy + dz*dz < distance_threshold){nr_good_fitts++;}
			}
			if(best_fit < nr_good_fitts)
			{
				best_fit = nr_good_fitts;
				nr_good_fitts = 0;
				for(int j = 0; j < nr_matches; j++)
				{
					src_point = src_match_id[j];
					dst_point = dst_match_id[j];
					
					float x = src_point->point->x;
					float y = src_point->point->y;
					float z = src_point->point->z;
				
					float dx = x*mat00+y*mat01+z*mat02+mat03-dst_point->point->x;
					float dy = x*mat10+y*mat11+z*mat12+mat13-dst_point->point->y;
					float dz = x*mat20+y*mat21+z*mat22+mat23-dst_point->point->z;
					if(dx*dx + dy*dy + dz*dz<distance_threshold){
						best_indexes[nr_good_fitts] = j;
						nr_good_fitts++;
					}
				}
			}
		}
		tfc.reset();
		transformation->matches_src = new KeyPoint*[best_fit];
		transformation->matches_dst = new KeyPoint*[best_fit];
		transformation->nr_matches 	= best_fit;
		for(int i = 0; i < best_fit; i++)
		{
			dst_point = dst_match_id[best_indexes[i]];
			src_point = src_match_id[best_indexes[i]];
			transformation->matches_src[i] = src_point;
			transformation->matches_dst[i] = dst_point;
			tfc.add(src_point->point->pos,dst_point->point->pos);
			if(debugg_PlanePointRansacMatcherI){
				//printf("match surf distance: %f\n",dst_point->surf_descriptor->distance(src_point->surf_descriptor));
				//cvLine(img_combine,cvPoint(src_point->point->w, src_point->point->h), cvPoint(dst_point->point->w+sx, dst_point->point->h), cvScalar(0, 255, 0, 0), 1, 8, 0);
			}
		}
		transformation->transformationMatrix = tfc.getTransformation().matrix();
		if(debugg_PlanePointRansacMatcherI){printf("best_fit: %i\n",best_fit);}
		transformation->weight = best_fit*30;
		//printf("weight: %f\n",transformation->weight);
	}else{
		transformation->transformationMatrix = Eigen::Matrix4f::Identity();
	}
	if(debugg_PlanePointRansacMatcherI){cvShowImage("combined image", img_combine);cvWaitKey(0);}
	transformation->next = 0;
	transformation->prev = 0;
	return transformation;
	*/
